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1.  INTRODUCTION 


The  work  described  in  this  report  has  to  do  with  the  problem  of  vision  or  camera-based 
navigation.  In  these  problems,  it  is  often  necessary  to  map  the  location  of  some  feature  or  object, 
which  is  subsequently  used  when  calculating  the  position  of  a  camera-based  navigation  system  user. 
Mapping  the  location  of  this  feature  relative  to  some  known  location  is  easy;  one  simply  needs  to 
know  a  direction  and  distance  from  the  known  point  to  the  feature  point.  However,  it  is  not  always 
practical  or  possible  to  measure  the  distance  to  an  object  (e.g.,  in  robotics,  finding  the  location  of  a 
visual  landmark  that  a  robot  can  see,  but  not  reach  with  a  laser  rangefinder  or  mechanical  arm).  All  is 
not  lost,  however.  It  is  possible  to  map  the  location  of  the  visual  landmark  if  the  robot  can  measure 
the  angle  towards  it  from  two  other  known  positions.  The  distance  to  the  landmark  can  be  deduced 
from  the  angles. 

Triangulation,  which  this  technical  report  discusses,  is  the  act  of  mapping  a  feature  using 
information  about  the  direction  towards  a  landmark  from  two  or  more  known  locations.  This  report 
focuses  on  a  method  of  solving  for  the  location  of  a  visual  landmark — a  feature  point  appearing  in 
two  or  more  images — ^using  a  linear  least  squares  algorithm.  Although  it  does  not  perform  as 
optimally  in  terms  minimizing  position  error  as  non-linear,  iterative  triangulation  algorithms',  the 
linear  least  squares  approach  is  fast,  fixed-cost,  and  suitable  for  machine  computation.  Furthermore, 
because  the  least  squares  algorithm  has  a  closed-form  solution,  it  simple  to  propagate  uncertainties  in 
camera  position  and  orientation  to  uncertainty  in  the  three-dimensional  (3-D)  location  of  the  feature 
point. 

The  approach  described  in  this  report  was  developed  to  triangulate  visual  landmarks  identified  in 
images  from  two  locations  taken  with  a  calibrated  camera.  This  report  assumes  the  locations  of  the 
camera  are  extracted  from  a  navigation  system,  which  gives  the  camera  position  in  the  north-east- 
down  (NED)  local  coordinate  frame  and  camera  orientation  in  the  so-called  “National  Aeronautics 
and  Space  Administration  (NASA)  Aerospace  3-2-1  Euler  angles”^.  The  result  is  a  closed-form 
equation  that  maps  16  variables  (roll,  pitch,  yaw,  NED  position  coordinates  for  each  camera  location; 
and  pixel  coordinates  for  each  image)  to  three  variables,  the  x,  y,  and  z  coordinates  of  the  feature 
point. 

This  report  also  presents  a  method  for  estimating  the  error  in  the  triangulation  solution  as  well  as 
an  evaluation  of  the  performance  of  the  error  estimation.  To  estimate  the  error  of  the  triangulation 
algorithm,  the  triangulation  equation  is  linearized  about  the  given  inputs  using  a  first-order  Taylor 
expansion.  The  triangulation  equation  must  be  linearized  because  the  values  used  in  the  linear-least 
squares  problem  are  non-linear  functions  of  the  camera  orientation.  The  linearization  allows  the  error 
covariance  of  the  16  input  variables  to  be  mapped  to  the  error  covariance  of  the  3-D  point  location. 
To  examine  the  performance  of  the  error  estimation  algorithm,  we  run  Monte  Carlo  simulations  with 
noisy  camera  positions  and  Euler  angles  and  compare  the  distribution  of  the  outputs  with  the 
expected  distribution.  The  result  is  an  estimate  of  the  visual  landmark’s  location  in  three  dimensions 
with  an  accurate  description  of  the  uncertainty  in  the  position.  With  the  triangulation  and  error- 
estimation  algorithms,  it  is  possible  to  accurately  locate  visual  landmarks  and  describe  the 
uncertainty  of  the  landmark’s  position  coordinates. 
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2.  TRIANGULATION  IN  A  EUCLIDEAN  FRAME 


We  will  first  demonstrate  a  method  for  solving  the  triangulation  problem  with  vector  algebra  in  a 
general  Euclidean  reference  frame,  and  then  show  how  these  vectors  can  be  extracted  from  camera 
and  navigation-filter  output.  The  triangulation  algorithm  first  uses  position  and  direction  information 
to  determine  the  distance  to  the  feature  point  from  each  known  location,  and  then  assigns  a  location 
to  the  feature  point  by  adding  these  distances  to  each  of  the  observation  locations. 

Phrasing  the  triangulation  problem  in  terms  of  vectors  is  straightforward.  To  locate  an  unknown 
point  from  two  known  observation  points,  four  pieces  of  information  are  needed:  the  location  of  each 
observation  point,  and  a  direction  from  each  observation  point  to  the  unknown  point. 

This  information  can  be  described  with  four  column  vectors:  Ti,  T2,  £  R^,  which  describe  the 
location  of  each  observation  point  in  the  navigation  frame,  and  di,  d2  £  R^,  which  are  vectors  that 
point  from  the  observation  point  to  the  landmark.  These  quantities  are  illustrated  in  Figure  1. 


Figure  1 .  The  variables  needed  for  the  triangulation  of  an  unknown  point  X. 

The  unknown  point,  X,  lies  at  a  distance  A,i  along  the  ray  defined  by  extending  the  vector  di  from 
the  point  Ti.  The  same  is  true  for  some  distance  X2  along  the  ray  defined  by  d2  and  T2.  The  unknown 
point  is  located  at  intersection  of  these  two  line-of-sight  vectors,  as  shown  in  Figure  2. 


Figure  2.  The  landmark,  X,  exists  at  the  intersection  of  rays  extended  from  each  observation  point 
(Ti  and  T2)  towards  the  landmark. 

This  geometric  construction  leads  to  the  equality: 

X  =  A^d^+^^=A2d2+^2.  (1) 
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Here,  the  terms  A,idi+Ti  and  A,2d2+T2  are  the  line-of-sight  vectors,  representing  a  ray  drawn  from 
each  observation  location  to  the  feature  point.  The  unknown  point,  X,  can  be  found  by  first  solving 
for  the  depth  scalars  Xi,  X2,  and  then  plugging  in  to  Equation  (1)  to  solve  for  X. 

2.1  SOLVING  FOR  DEPTH 

Rearranging  Equation  (1)  gives  an  equation  that  can  easily  solve  for  depth; 

;iidi-l2d2  =T2-Ti.  (2) 

For  convenience,  T  =  T2  -  Ti.  Using  this  definition.  Equation  (2)  above  can  be  written  as  the 
following  vector  equation: 


[d,  -O2] 

Solving  this  using  linear  least  squares  will  yield  the  depths  Xi,  X2  that  minimize  the  distance 
between  A,idi-X2d2,  and  T.  In  other  words,  this  attempts  to  form  a  triangle  with  sides  A-idi,  A,2d2,  and 
T,  as  shown  in  Figure  3. 


^idi 


Figure  3.  The  linear  least  squares  algorithm  finds  the  Ai,  A2  such  that  the  vectors  Aidi,  A2d2,  and  T 
form  a  triangle. 

Solving  Equation  (3)  using  linear  least  squares  leads  to  the  expression; 


(3) 

=  T. 


A, 


did, 


diT 

-diT 


(4) 


With  this  closed-form  solution  for  the  depths,  the  location  of  the  unknown  point  X  can  be  easily 
found  by  back  projecting  each  depth  from  each  observation  point.  Rearranging  Equation  (1)  to 
illustrate  the  effect  of  both  depths  on  the  solution  leads  to 

^  _  ^d,  -I-  T,  ^  ^2^2  +  T2  (5) 

~  2  2  ■ 
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In  two-dimensions,  this  solution  is  identical  to  solving  for  X  using  only  one  observation  direction 
and  one  depth.  The  next  section  discusses  the  purpose  and  geometric  meaning  of  the  average  of  the 
two  solutions. 

2.2  THE  EFFECT  OF  LINEAR  LEAST  SQUARES  ON  THREE-DIMENSIONAL  TRIANGULATION 

In  two  dimensions,  the  rays  from  each  observation  always  intersect  if  they  are  not  parallel.  In 
three  dimensions,  due  to  noise,  the  rays  usually  not  intersect.  However,  as  long  as  the  two  rays  are 
not  parallel,  the  least  squares  algorithm  described  above  will  give  a  solution  that  can  be  used  as  an 
estimate  of  the  feature  point’s  location.  In  what  follows,  we  describe  the  geometric  meaning  of  this 
estimate. 

The  solution  generated  by  the  linear  least  squares  algorithm  is  the  point  where  the  lines  almost 
intersect.  To  be  more  precise,  the  position  estimate  X  from  Equation  (5)  gives  the  mid-point  of  the 
shortest  line-segment  that  connects  the  two  line-of-sight  vectors,  A,idi+Ti  and  X2d2+T2.  This  is  best 
illustrated  by  demonstrating  which  norm  is  minimized  by  the  least  squares  estimate  for  depth  in 
Equation  (4). 


Figure  4.  The  linear  least  squares  algorithm  will  find  a  Ai  and  A2  that  minimize  the  magnitude  of  the 
expression  |Ai  di  -  A2  d2  -  T|. 

As  seen  in  Figure  4,  the  least  squares  method  calculates  the  depths  that  minimize  the  error 
between  the  vector  difference,  di  -  A2  d2,  and  the  estimate  of  camera  translation,  T.  This  is 
equivalent  to  calculating  the  projection  of  T  onto  the  plane  defined  by  di  and  d2.  When  these  depths 
are  independently  used  in  reconstruction  from  each  view,  the  two  reconstructions  are  inconsistent,  as 
shown  in  Figure  5. 


Figure  5.  The  3-D  reconstructions  from  back-projecting  each  view  may  be  slightly  different.  The  final 
estimate  of  the  point  will  be  the  average  of  the  two  back-projections. 
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Averaging  the  results  of  the  two  back-projections  produces  a  solution  for  the  unknown  position. 
We  will  use  this  solution  as  our  final  estimate  of  the  feature  point’s  position.  This  algorithm  can  now 
be  used  in  general  to  triangulate  a  3-D  location.  Adapting  this  algorithm  for  use  with  cameras  and 
navigation  filters  will  require  models  for  camera  behavior  and  extracting  Euclidean  camera  location 
and  orientation  from  navigation  filter  outputs. 

3.  ADAPTING  TO  A  PINHOLE  CAMERA  MODEL  AND  NAVIGATION 
FILTER 

As  we  have  seen,  the  only  information  needed  to  triangulate  the  location  of  an  unknown  point  is 
the  line-of-sight  vectors  toward  the  feature  point  from  two  known  locations.  This  information  can  be 
extracted  from  the  output  of  a  navigation  filter.  The  location  of  each  observation  point  is  the  position 
output  of  the  navigation  filter  at  the  time  an  image  was  taken,  corrected  for  lever-arm  effects  if  the 
camera  is  not  co-located  with  the  navigation  system.  Determining  the  line-of-sight  vector  in  the 
navigation  frame  is  more  complicated  because  it  requires  knowledge  of  the  attitude  of  the  observer, 
lever-arm  effects,  and  camera  calibration.  Three  transformations  are  required  for  mapping  a  feature’s 
image  coordinates  to  a  3-D  line-of-sight  vector  in  the  navigation  frame: 

1 .  A  transformation  to  convert  a  feature’s  u,v  pixel  coordinates  to  a  3-D  line-of-sight 
vector  in  the  camera’s  frame  of  reference. 

2.  A  transformation  to  map  this  3-D  line-of-sight  vector  from  the  camera’s  frame  of 
reference  to  the  navigating  body’s  frame  of  reference. 

3.  A  transformation  to  map  the  3-D  line-of-sight  vector  from  the  body  frame  to  the 
navigation  frame.  After  this  transformation,  the  line-of-sight  vector  points  from  the 
observation  point  towards  the  feature  point’s  location  in  the  navigation  frame. 

This  process  is  summarized  in  Figure  6. 


Figure  6.  The  transformations  needed  to  reach  the  navigation  frame  from  the  camera  frame. 
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While  describing  the  transformation  from  pixel  coordinates  to  vectors  in  the  navigation 
coordinates,  several  placeholder  variables  and  measured  matrices  will  be  used.  For  reference,  these 
variables  are  summarized  in  Table  1. 


Table  1.  Variables  used  in  deriving  triangulation  and  covariance  equations. 


w 

Vector  representing  the  u,v  pixel  coordinates  of  a  feature  point 

X 

Vector  representing  the  line-of-sight  vector  in  the  camera  frame. 

b 

Vector  representing  the  line-of-sight  vector  in  the  body  frame. 

d 

Vector  representing  the  line-of-sight  vector  in  the  navigation  frame 

Rotation  matrix  used  to  rotate  the  vector  x  from  the  camera  frame 
to  the  body  frame 

Rotation  matrix  used  to  rotate  the  vector  b  from  the  body  frame  to 
the  navigation  frame 

N 

Vector  representing  the  position  output  by  the  navigation  filter 

L 

Vector  representing  the  lever  arm  from  the  user  to  the  camera 
center,  in  the  body’s  frame  of  reference 

T 

Vector  representing  the  location  of  the  camera  center  in  the 
navigation  frame 

3.1  PINHOLE  CAMERA  AND  CAMERA  CALIBRATION  MODEL 

The  pinhole  camera  model  is  the  classic  model  used  to  map  a  feature’s  3-D  coordinates  to  2-D 
pixel  coordinates^.  Its  inverse  can  therefore  be  used  to  convert  a  feature’s  pixel  coordinates  to  a 
vector  that  is  consistent  with  the  Euclidian  coordinates  of  the  navigation  frame.  As  shown  in  Figure 
7,  the  pinhole  camera  model  maps  3-D  points  [x,  y,  z]^  in  the  camera’s  frame  of  reference  to  2-D 
coordinates  on  an  image. 


Figure  7.  The  pinhole  camera  model  projects  3-D  points  onto  a  2-D  plane  (figure  from  Equation  [2]). 
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To  use  pixel  coordinates  for  triangulation,  the  calibration  matrix  of  the  camera  that  took  the 
image  must  be  known.  The  camera  calibration  matrix  is  used  when  modeling  the  projection  of  a 
scene  onto  an  image  sensor.  This  projection  can  be  modeled  by: 


fx  skew  c, 

fy  S 


Au 

X 

Av 

=  K 

y 

A 

z 

(6) 


(7) 


u 

1 

X 

V 

=  -K 

y 

A. 

1 

z 

(8) 


Here,  u,v  are  the  pixel  coordinates  of  a  feature  point,  K  is  the  camera  calibration  matrix,  [x,y,z]^ 
are  the  coordinates  of  the  feature  point  in  the  camera’s  frame  of  reference,  and  X  is  the  depth  scalar  in 
the  same  units  as  the  navigation  frame. 

From  Equation  (8),  we  can  recover  a  vector  x,  which  points  in  the  direction  of  the  feature  point  in 
the  camera’s  frame  of  reference.  This  vector  can  also  be  described  as  the  calibrated  image  of  the 
feature  point. 


w  = 


X 


y 


=  AK  'w. 


z 


(9) 


(10) 


x  =  K-‘w.  (11) 

The  vector  x  is  a  vector  in  the  camera’s  frame  that  points  towards  a  feature  point,  meeting  it  at  a 
depth  X.  This  is  shown  in  Figure  8. 

Because  of  the  camera  calibration  matrix,  this  X  is  consistent  with  the  depth  that  is  solved  for  in 
the  linear  least  squares  problem  above.  Thus,  an  image  coordinate  can  be  mapped  from  the  [u  v  1]^ 
coordinate  system  to  a  vector  x  in  the  camera  frame  that  is  now  related  to  the  navigation  frame  by  a 
simple  rigid  body  transformation.  This  x  can  be  then  transformed  to  the  navigation  frame  to  find  the 
vector  d  that  is  needed  for  triangulation. 
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Figure  8.  The  vector  x  from  Equation  (1 1 )  points  toward  the  feature  point  in  the  camera  frame  of 
reference. 

3.2  FROM  THE  CALIBRATED  IMAGE  TO  THE  BODY  FRAME 

The  calibrated  image  vector  x  now  needs  to  be  aligned  to  the  navigation  frame  in  order  to 
triangulate  the  feature  point.  The  navigation  filter’s  estimate  of  attitude  can  be  used  to  align  the 
camera  frame  with  the  navigation  frame.  However,  the  attitude  reported  by  the  filter  is  the  orientation 
of  some  rigid  body  frame  with  respect  the  navigation  frame,  and  this  frame  is  not  usually  the  same  as 
the  camera  frame.  Therefore,  the  calibrated  image  vector  from  the  previous  step  has  to  be 
transformed  to  the  user’s  body  frame  before  being  transformed  to  the  navigation  frame. 

The  transformation  from  camera  frame  to  body  frame  is  a  simple  rigid  body  transformation,  i.e., 
it  consists  of  a  translation  and  rotation.  Here  we  will  call  the  translation  vector  L,  and  the  rotation 
matrix  ,  and  their  effect  is  shown  in  Figure  9. 


Figure  9.  The  camera  frame  is  translated  and  rotated  away  from  the  body  frame. 

The  vector  L  represents  the  lever  arm  between  the  camera  center  and  the  point  whose  location  is 
output  by  the  navigation  fdter.  The  vector  L  is  defined  in  the  body  frame  and  thus  its  effect  on  the 
camera’s  location  in  the  navigation  frame  can  easily  be  calculated.  The  lever  is  used  to  extract  the 
camera’s  location  from  the  location  of  the  navigation  body.  For  the  purposes  of  this  paper,  it  is 
assumed  that  the  matrix  and  lever  arm  L  are  error-free  and  known.  With  this  information,  the 
vector  X  from  the  previous  step  can  be  transformed  to  a  vector  in  the  user’s  body  frame. 


Converting  from  camera  frame  to  body  frame  can  be  accomplished  using  the  following  formula; 

b=C^x.  (12) 

The  calibrated  image  vector  is  now  expressed  in  the  body  frame.  The  lever  arm  L  will  be  used 
later  to  define  the  observation  position. 

3.3  EULER  ANGLES  AND  DIRECTION  COSINE  MATRIX 

The  last  step  before  triangulation  can  be  completed  is  transforming  the  vector  b  from  the  body 
frame  to  the  navigation  frame  to  get  the  line-of-sight  vector  Xidi+Ti.  This  is  accomplished  using 
information  from  the  output  of  the  navigation  filter.  This  report  assumes  that  the  navigation  filter 
outputs  orientation  in  Euler  angles  in  the  NASA  3-2-1  aerospace  sequence  yaw  (ip),  pitch  (0),  roll 
(O)^ 

The  transformation  from  body  frame  to  navigation  frame  is  accomplished  using  a  3  x  3  rotation 
matrix,  here  called  Cl .  This  rotation  matrix  is  a  function  of  the  Euler  angles  as  shown  below,  where 
c  and  s  are  used  as  shorthand  for  cosine  and  sine: 


C"  = 


With  a  simple  matrix  multiplication,  vectors  in  the  body  frame  are  transformed  to  the  navigation 
frame. 


S^SgS^+C^C^  C^SgS^-S^C^ 


-Sr 


(13) 


d=C^b.  (14) 

The  Cl  matrix  is  also  used  to  solve  for  the  position  of  the  camera  at  the  time  the  image  was 

taken.  The  navigation  filter  outputs  a  position  solution  as  a  3 -vector,  and  the  position  of  the  camera  is 
simply  the  output  of  the  navigation  filter,  Ni,  plus  the  rotated  body-frame  components  of  the  lever 
arm  L.  Therefore,  we  can  write; 

Ti=Ni+C”L.  (15) 

We  now  have  all  the  information  needed  to  compute  the  depth  of  the  feature  point,  and  thus,  the 
3-D  location  of  the  feature  point. 

3.4  CLOSED-FORM  SOLUTION  FOR  THE  DIRECTION  RAYS,  CAMERA  POSITION  VECTORS, 
AND  LANDMARK  DEPTH 

Combining  all  of  the  above  steps  into  a  single  equation  for  the  direction  vectors  and  location 
vectors,  we  get 

di=c;;c^K"V.  (16) 
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The  values  from  Equations  (1 1),  (15),  and  (16)  can  be  substituted  into  Equation  (4)  to  calculate 
an  estimate  for  the  depths  Xi,  Xi.  One  important  observation  is  that  the  magnitude  of  the  direction 
vectors  di  and  di  only  depend  on  K  and  the  image  coordinates  Wi  and  W2,  i.e.,  the  magnitude  of  di 
equals  the  magnitude  of  Xi: 

||dJ|=|k||.  (17) 


Thus,  Equation  (4)  can  be  simplified  into  the  following,  which  will  simplify  deriving  the 
equations  for  estimating  the  error  of  this  triangulation  algorithm; 


didi 


d^T 

-diT 


(18) 


The  partial  derivatives  of  this  equation  contains  fewer  terms  because  the  Xj  terms  do  not  depend 
on  cl  or  Cl .  It  is  therefore  easier  to  take  the  Jacobian  of  this  expression  to  estimate  the  error 
covariance  of  the  solution. 


4.  ESTIMATING  THE  QUALITY  OF  THE  SOLUTION 

One  way  to  characterize  the  quality  of  the  triangulation  solution  is  to  calculate  a  covariance  of 
the  estimation  errors  as  a  function  of  pixel  noise,  camera  position  error,  and  camera  attitude  error. 
Since  the  relationship  between  X  error,  pixel  error,  and  camera  attitude  error  is  non-linear,  this  will 
require  a  linearization  of  the  triangulation  equation.  If  the  estimate  of  the  landmark  positions,  X, 
generated  by  the  triangulation  method  outlined  above  is  to  be  used  for  navigation,  tracking,  or 
surveying,  we  will  need  to  know  if  our  estimate  of  X  is  accurate  and  be  able  to  quantify  that 
accuracy.  The  classic  way  of  estimating  the  error  covariance  for  a  variable  described  by  a  non-linear 
function  like  the  triangulation  algorithm  is  through  a  first-order  Taylor  series  expansion. 

The  first-order  Taylor  series  expansion  treats  each  x,  y,  and  z  component  of  the  position  estimate 
X  as  a  continuous,  differentiable  function  of  several  input  variables.  The  input  variables  will  depend 
on  the  individual  application.  A  first-order  Taylor  series  expansion  allows  for  an  estimate  of  how 
errors  in  each  of  those  input  variables  contribute  to  errors  in  the  x,  y,  and  z  positions  of  the  position 
estimate.  As  mentioned  in  Section  3.3,  it  is  assumed  the  camera  calibration  matrix  K,  the  camera  to 
body  transform  ,  and  the  lever-arm  L  are  known  and  error-free.  Thus,  this  section  assumes  that 

there  will  be  errors  in  the  camera  positions,  camera  attitudes,  and  the  pixel  coordinates  of  the 
landmark  in  each  image.  The  camera  position  and  attitude  will  come  from  a  navigation  filter  that 
outputs  a  position  solution  in  NED  coordinates  and  orientation  in  3-2-1  NASA-aerospace  roll-pitch- 
yaw  Euler  angles.  Therefore,  each  observation  of  the  landmark  will  have  eight  sources  of  error 
(errors  in  north,  east,  down,  roll,  pitch,  yaw,  u,  and  v  coordinates).  Thus,  the  landmark  position  X  is 
estimated  from  two  observations  that  each  has  eight  sources  of  error.  This  results  in  X  being 
described  as  a  function  of  the  following  16  variables; 

X  ^(  Aj ,  E'j,  Dj ,  ,  t/j ,  2  9  -^2’  7^2  ?  ^2’  ^2  ’  1^2’  ^2’  * 
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Taking  the  partial  derivatives  of  these  functions  forms  a  Jacobian  matrix,  and  multiplying  this 
Jacobian  with  the  prior  estimates  of  error  in  each  of  the  16  variables  produces  a  reliable  estimate  of 
the  error  of  the  triangulation  solution. 

The  Jacobian  of  X,  which  will  be  referred  to  as  J ,  is  a  3x16  matrix  with  the  form: 


'sx 

sx 

sx 

sx 

SX 

sx 

sx 

sx 

sx 

SX 

SX 

SX 

SX 

sx 

sx 

sx' 

SE, 

SD^ 

5<k 

se, 

Su^ 

dv^ 

SN^ 

SE^ 

SD^ 

s<k 

se. 

Sif/^ 

Su2 

dX  =  J& 


(19) 


The  covariance  estimation  will  be  performed  by  the  standard  covariance  transformation 
equation^: 


Px  =E|x-xXx-xf  }=e{(5X(5X'^}  (20) 

=  e{j&&'^j'^} 

=  je{&&'^)j'^ 

=  JO  J^, 

where  E{}  is  the  expectation  operator,  is  the  covariance  of  the  point’s  location,  and  O  is  the 
16x16  covariance  matrix  of  the  16  input  variables  that  represent  the  camera’s  positions  and  the  visual 
landmark’s  pixel  coordinates. 

4.1  DERIVING  THE  POSITION  JACOBIAN 

Calculating  J  can  be  difficult,  and  to  simplify  the  calculations  we  make  the  following 
observations.  From  the  position  solution  Equation  (5),  we  see  that  the  position  solution  is  mainly  a 
function  of  six  variables:  X,  d  and  T  for  each  of  the  two  viewpoints.  We  will  take  advantage  of  this 
and  use  the  chain  rule  to  simplify  the  derivation  of  the  Jacobian.  Furthermore,  symmetry  in  equation 
(5)  also  simplifies  the  derivation  of  the  Jacobian.  That  is,  the  term  dj  +Tj  appears  twice.  This 
allows  us  to  express  the  Jacobian  as  the  sum  of  two  matrices: 

J  J1+J2  (21) 

2 

Here,  Jj  and  J2  represent  the  Jacobians  of  the  terms  rj=/ljdj+Tj  and  r2=22d2+T2, 
respectively.  Each  Jacobian  J,  contains  the  partial  derivatives  of  Tj  with  respect  to  the  16  input 
variables: 


Sr, 

Sr., 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

Sr, 

_5N^ 

SD, 

S^, 

SO, 

Sij/, 

Su, 

Sv, 

5N^ 

<©2 

S<l>2 

se^ 

SVi 

SlI'2 

Sv2 
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The  Jacobian  of  the  expression  /I,  dj  +Tj  is  found  by  taking  the  partial  derivatives  with  respect  to 
the  16  input  scalars  in  z.  The  partial  derivative  Tj  with  respect  to  any  scalar  variable  a  is: 

SI  ,  ,  ST,  (22) 

— '-  =  — '-d,  +  A, — '-  +  — 

Sa  Sa  Sa  Sa 


Next,  the  partial  derivatives  of  di,  T;  ,and  are  determined.  Recall  from  Equation  (16)  that  the 
vector  di  is  function  of  several  change  of  bases  operations  on  an  image’s  u,v  coordinates.  The  partial 
derivatives  of  the  depth  vector  dj  are  therefore. 


Sa  Sa 
+  C 


Sa 


K  *w,- 


+  C 


SK-'  ,,  I  Sy/. 

- Wj  +K  — 

Sa  Sa 


JJ 


(23) 


For  this  report,  the  camera-to-body  transformation  matrix  and  the  calibration  matrix  K  are 
assumed  to  be  known  and  invariant,  so  Equation  (23)  simplifies  to 


Sd, 

Sa 


SCI 

Sa 


C!K 


-1. 


Wi 


(24) 


With  the  partial  derivatives  of  dj  defined,  the  next  terms  of  Equation  (22)  to  define  are  the  partial 
derivatives  ofTj .  From  Equation  (15),  we  can  see  that  Tj  depends  on  CJJ ,  L,  and  Nj.  The  partial 

derivatives  ofTj  are  therefore, 


Sa  Sa  Sa  ‘  Sa 

The  lever-arm  L  is  assumed  known  and  invariant,  so  Equation  (25)  simplifies  to 

SN,  SCI  T 
Sa  Sa  Sa 

The  partial  derivatives  of  the  depths,  A,i,  are  calculated  in  a  similar  fashion  to  the  partial 
derivatives  for  the  terms  of  dj  and  T; .  However,  Equation  (18)  is  more  complex  than  the  one  used  for 
the  position  equation.  To  simplify  the  process  of  taking  the  partial  derivatives  of  this  equation,  we 
will  take  advantage  of  the  division  rule  of  derivatives.  That  is,  we  will  separate  the  expression  in 
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Equation  (18)  into  two  parts,  the  numerator  and  denominator  and  combine  the  partial  derivatives  of 
these  parts; 


(27) 


(28) 


We  need  the  partial  derivatives  of  the  placeholder  variables  high  and  low  to  compute  the  partial 
derivatives  of  the  depths.  These  partial  derivatives  are 
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The  terms  Xi  and  X2  in  Equations  (27)  through  (30)  are  the  calibrated  image  vectors  from 
Equation  (11).  The  partial  derivatives  of  Xi  are 

=  (31) 

Sa  Sa 


With  these  derivatives  of  the  intermediate  variables  di,  Tj  ,and  Xi  defined,  only  the  partial 
derivatives  of  Wj ,  Nj ,  and  CJ”  need  to  be  found.  The  variables  Wj ,  Nj ,  and  are  functions  of  the 
input  variables  only.  Their  partial  derivatives  are 


(JWj 


1 

0 

0 

0 

1 

0 


a  =  Ui 


a  =  Vi 


0,  otherwise 


(32) 


1 

0 

0 

0 

1 

0 

o' 

0 

1 


a  =  A^i 

a  =  E, 

a=D- 


0,  otherwise 


(33) 


0 
0 


-  s.s.r  -  c^c,„ 


'-(p'-e'-ij/ 

-c.s, 


<l>^0 


- 

^ ij/  ~  ^ (j)^ y/  ^ (j)^ 0^ y/  ^ y/ 

0  0 


a  =  \\f- 


0, 


otherwise 


(34) 
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With  these  partial  derivatives  defined,  it  is  now  possible  to  construct  the  Jacobian  matrices  Jj 
and  J  2 ,  and  use  them  in  Equation  (21)  to  determine  J .  The  Jacobian  J  is  now  ready  to  be  used  to 
calculate  the  covariance  of  the  feature  point’s  location  using  Equation  (20).  However,  before  that 
calculation  can  be  performed,  the  input  covariance  matrix,  n ,  must  be  defined. 

4.2  DEFINING  THE  INPUT  COVARIANCE  MATRIX 

The  input  covariance  matrix  contains  the  variances  and  co-variances  of  the  16  input  variables.  It 
is  used  to  calculate  a  feature  point’s  3x3  position  covariance  matrix  after  triangulation. 

The  input  covariance  matrix  (Figure  10)  is  a  16x16  positive-definite  symmetric  matrix.  The  16 
input  variables  are:  the  camera  position  for  each  observation  (in  NED  coordinates),  the  camera 
orientation  for  each  observation  (in  roll-pitch-yaw  Euler  angles),  and  the  feature  position  in  each 
image  (in  u  and  v  pixel  coordinates). 


Figure  10.  The  shape  and  contents  of  the  input  covariance  matrix.  Regions  1  and  3  contain  the  NED, 
phi,  theta,  psi  covariances  for  the  first  and  second  images,  respectively.  Regions  2  and  4  contain  the 
u,v  covariances  for  images  1  and  2. 

In  Figure  10,  the  16x16  input  covariance  matrix  is  divided  into  four  regions.  Regions  1  and  3  are 
the  covariances  of  the  outputs  of  the  navigation  filter.  Regions  2  and  4  are  the  covariances  of  the 
feature  location,  in  pixels.  Region  1  is  a  6x6  covariance  matrix  containing  entries  from  a  navigation 
system’s  estimate  of  error: 


15 


where  E  is  the  expectation  operator.  Similarly  region  3  is  defined  as 


^9:14,  9:14 


fr 

SE2SN2 

SD2SN2 

S^2^2 

862^^2 
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\ 

SE^SN^ 

SD2SE2 

<5^2<®2 

882^2 

811/2^2 

SD^SN^ 

SD2SE2 

S^2^2 

8628D2 

8X1/28D2 

S(p23N^ 

(<V,t 

8628^2 

8x1/28^2 

se^SN^ 

^6>2®2 

m 
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SI//2SE2 

SI//2SD2 

5WiS<l>i 

8x1/2862 

{SWiY  _ 
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The  expected  values  for  each  of  these  variables  are  obtained  from  the  covariance  estimate  that  is 
output  by  the  navigation  filter. 

Regions  2  and  4  in  Figure  10  contain  the  uncertainty  in  the  feature  point’s  u,v  coordinates  for 
each  picture.  These  values  are  characteristic  of  the  feature  point  detection  algorithm. 

Region  2  is  defined  as 


^7:8,7:8  “ 


VL 


A 

J 


Similarly,  Region  4  is  defined  as 


^15:16,15:16 


(^2)^ 


J 


With  these  regions  of  Q  defined,  they  must  be  populated  with  actual  values  before  Equation  (20) 
can  be  used  to  calculate  .  The  values  for  each  entry  in  regions  1  and  3  are  output  from  the 
navigation  filter,  and  regions  2  and  4  are  defined  by  the  choice  of  feature  point  detection  algorithm. 
Once  n  is  complete,  then  J  and  can  be  used  in  Equation  (20)  to  solve  for  .  With  this  P^  and 

X  from  Equation  (5),  we  now  have  an  estimate  for  the  position  of  the  feature  point  and  an  estimate  of 
the  uncertainty  of  the  position. 


5.  ESTIMATING  PROJECTION  ERROR 

In  addition  to  estimating  a  landmark’s  coordinates  from  a  sequence  of  images,  we  can  estimate 
where  in  an  image  a  landmark  should  appear  given  its  coordinates  and  an  estimate  of  the  camera’s 
location.  That  is,  using  the  pinhole  camera  model,  we  can  predict  the  pixel  coordinates  of  a  visual 
landmark.  More  formally,  given  a  landmark’s  position  in  the  navigation  frame  as  well  as  estimates  of 
the  camera’s  position  and  attitude,  we  can  estimate  and  Vj ,  the  pixel  coordinates  of  a  feature 
point,  and  P^^ ,  the  error  covariance  of  the  estimation  of  the  pixel  coordinates.  Combined  with 

estimates  of  the  error  in  the  camera  position  and  landmark  location,  this  pixel  error  estimate  can  be 
used  for  feature  matching,  feature-point  integrity  algorithms,  or  tightly  coupled  visual  navigation. 


16 


We  will  first  derive  a  closed-form  equation  for  estimating  the  pixel  coordinates  of  a  feature  point, 
and  then  use  the  Jacobian,  ,of  this  closed- form  equation  to  estimate  the  error  covariance  matrix. 

To  estimate  a  visual-landmark’s  pixel  coordinates,  one  simply  rearranges  the  triangulation  equations 
from  Equation  (1)  to  obtain 


dj 


^(X-T). 


(35) 


Substituting  the  results  from  Equations  (16)  and  (17)  obtains 

qc,^K-y  (X-T). 


(36) 


Rearranging  Equation  (36)  to  solve  for  u  and  v  results  in 


w,  = 


=  ^K(qf(qf(X-T). 


(37) 


Here,  kj  is  the  predicted  depth  of  the  landmark  in  the  scene.  It  is  obtained  through  a  similar 
equation: 

kj  =  [o  0  i]k(c^  f  (c;;  f (X;  -  t). 


We  now  have  a  simple  set  of  equations  for  estimating  the  pixel  coordinates  of  a  feature  point. 
These  equations  can  also  be  used  to  define  an  estimate  of  the  error  of  the  projection. 

The  method  used  to  estimate  the  error  is  the  same  as  in  Section  4 — the  first-order  Taylor 
expansion  method  of  linearization.  To  simplify  the  partial  derivatives  of  the  projection  equation 

described  in  Equation  (37),  we  first  note  that  both  the  equations  for  ,  and  contain  the 
expression  k(c*)^  (Cj)^(X-T)  .  We  will  call  this  expression  h ,  as  it  represents  the  homogenous 
coordinates  of  the  feature  point  on  the  imaging  plane^: 

h  =  K(qf(c;:f(X-T).  (39) 


With  the  homogenous  coordinates  defined,  we  can  express  the  pixel  coordinates  and  depth  of  a 
feature  point  as  a  function  of  h : 


Wj 

Vi 

1 


=  [o  0  l]  h;. 


(40) 


(41) 


It  is  easy  to  take  the  partial  derivatives  of  these  expressions  to  use  in  the  calculation  of  the 
Jacobian  matrix: 
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(42) 
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With  the  partial  derivatives  of  the  pixel  coordinates  and  depth  derived  in  terms  of  the 
homogenous  image  coordinates,  we  now  need  to  calculate  the  partial  derivatives  of  the  homogenous 
image  coordinates; 
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(44) 


The  partial  derivatives  of  K  ,  and  T  are  the  same  as  the  ones  derived  in  Section  4.1. 

C* 

Under  the  assumptions  of  a  constant  lever  arm,  camera  calibration,  and  ,  the  pixel  coordinates  of 
a  feature  points  are  a  function  of  nine  variables;  the  world  coordinates  of  the  feature  point(  three 
degrees  of  freedom),  and  the  position  and  orientation  of  the  camera  (six  degrees  of  freedom).  With  a 

NED  coordinate  frame  and  3-2-1  NASA  aerospace  Euler  angles,  the  Jacobian  matrix  can  be 
defined  as  follows; 
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J 


uv 


JWj  JWj  JWj  JWj  JWj  JWj  AWj  ^Wj  JWj 

f  SEj  SDj^  S^f  S6j  Si//j-  SN-^  SE^  SD^ 

1  0  0l 


(45) 


Here,  the  subscript /indicates  variables  from  the  navigation  filter’s  current  position  and  attitude 
estimates  and  the  subscript  i  represents  the  world  coordinates  of  the  ith  feature  point.  Note  that 

Wj  =  [i/j  Vj  l]  ^  is  a  3-vector,  with  a  constant  third  entry,  and  therefore  the  third  row  of  J„  is 


removed  by  multiplying 


1  0  0 
0  1  0 


by  J„. 
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With  this  Jacobian  defined,  the  covariance  of  the  projection  can  be  calculated  using  the  standard 
method: 


P  =J  F  (J  f.  (46) 

Here,  is  the  predicted  covariance  matrix  for  the  uv  pixel  errors,  and  is  the  9x9  covariance 

matrix  of  the  nine  input  variables  (camera  position,  camera  orientation,  and  landmark  location).  The 
performance  of  this  covariance  estimator  will  be  discussed  in  Section  Error!  Reference  source  not 
found.. 


6.  EVALUATION  OF  PERFORMANCE 

With  algorithms  to  estimate  errors  in  landmark  triangulation  or  landmark  projection  defined,  we 
must  now  test  how  accurately  the  algorithms  estimate  the  magnitude  and  direction  of  errors.  To 
evaluate  the  performance  of  the  triangulation  error  covariance  estimate,  a  Monte  Carlo  simulation 
was  run  on  the  triangulation  algorithm.  The  algorithm  was  given  inputs  of  known  variance.  The 
distribution  of  the  outputs  of  the  triangulation  algorithm  was  compared  to  the  distribution  described 
by  the  output  of  the  covariance  estimation  algorithm.  The  variances  of  the  inputs  were  changed  to 
illustrate  the  effect  of  camera  position  noise  and  camera  orientation  noise  separately. 

6.1  TRIANGULATION  SIMULATION  SET-UP 

The  Monte  Carlo  simulation  was  run  assuming  a  system  with  two  cameras  placed  side-by-side 
and  triangulating  a  feature  point.  The  experiment  set-up  is  shown  in  Figure  11. 

The  two  cameras  were  facing  west,  with  a  feature  point  located  approximately  50  meters  to  the 
west.  The  cameras  were  spaced  10  meters  apart  in  the  north  direction,  and  the  feature  point’s 
projection  was  calculated  using  the  same  camera  calibration  matrix  for  each  camera.  The  exact 
parameters  are  listed  in  Table  2. 


T. 


Figure  11.  The  Monte  Carlo  simulation  set-up. 
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Table  2.  Mean  values  used  for  Monte  Carlo  simulation. 


Feature 

Point 

Location 

X 

(NED) 

3.14 

2.718 

-1.414 

meters 

c: 

'0  0  r 

1  0  0 

0  1  0 

(dimensionless) 

K 

2136.9  0  475.1 

0  2133.2  560.3 

0  0  1 

(dimensionless) 

0  1  0 

-10  0 
0  0  1 

(dimensionless) 

T, 

-5 

50 

_0_ 

(meters) 

Tz 

■5" 

50 

0 

(meters) 

The  simulation  was  run  under  12  different  configurations  with  varying  magnitudes  of  position, 
attitude,  and  pixel  error.  Each  run  consisted  of  100,000  trials.  The  noise  magnitudes  were  set  up  as 
indicated  inTable  3.  The  standard  deviations  were  the  same  for  camera  positions  and  orientations. 

The  performance  of  the  error  covariance  estimation  was  evaluated  by  calculating  the 
Mahalanobis  distance  of  the  error  of  each  trial  in  each  run.  The  Mahalanobis  distance  is  calculated  by 

comparing  the  observed  error,  X  -  X ,  with  the  expected  error  as  predicted  by  the  covariance  matrix. 
The  Mahalanobis  distance,  m  ,  is  calculated  using  the  following  formula: 

m^=(x-xf(pj‘(x-x).  (47) 

Here,  is  the  3x3  covariance  matrix  calculated  using  Equation  (20),  X  is  the  output  of  the 
triangulation  algorithm  given  the  noisy  inputs,  and  X  is  the  true  landmark  location. 

The  Mahalanobis  distance  is  a  dimensionless  distance  that  follows  a  chi-square  distribution  with 
the  same  number  of  degrees  of  freedom  as  the  covariance  matrix^.  Thus,  the  performance  of  a  three- 
dimensional  position  estimate  was  evaluated  by  comparing  the  distribution  of  the  Mahalanobis 
distances,  m  ,  of  the  errors  with  the  expected  3-degree-of-freedom  -distribution. 
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Table  3.  Test  Conditions. 


Run 

Variables 

Standard  Deviation 

A. 

Roll, pitch, yaw 

0.01  degrees 

N,E,D 

1  meter 

B. 

Roll,pitch,yaw 

0.01  degrees 

N,E,D 

5  meters 

C. 

Roll,pitch,yaw 

0.01  degrees 

N,E,D 

10  meters 

D. 

Roll,pitch,yaw 

1  degree 

N,E,D 

1  meter 

E. 

Roll,pitch,yaw 

1  degree 

N,E,D 

5  meters 

F. 

Roll,pitch,yaw 

1  degrees 

N,E,D 

10  meters 

G. 

Roll,pitch,yaw 

5  degrees 

N,E,D 

1  meter 

H. 

Roll,pitch,yaw 

5  degrees 

N,E,D 

5  meters 

1. 

Roll,pitch,yaw 

5  degrees 

N,E,D 

10  meters 

J. 

Roll,pitch,yaw 

10  degrees 

N,E,D 

1  meter 

K. 

Roll,pitch,yaw 

10  degrees 

N,E,D 

5  meters 

L. 

Roll,pitch,yaw 

10  degrees 

N,E,D 

10  meters 

6.2  TRIANGULATION  SIMULATION  RESULTS 

The  results  from  the  Monte  Carlo  simulation  are  shown  in  Figure  12.  The  results  from  each  of  the 
twelve  runs  are  arranged  to  show  how  the  performance  varies  as  a  function  of  position  noise 
magnitude  and  angle  noise  magnitude. 

Figure  12  contains  a  top-down  view  of  the  distribution  of  points.  The  largest  uncertainty  is  in  the 
direction  perpendicular  to  the  camera  plane.  This  is  because  the  errors  in  the  locations  and 
orientations  of  the  two  cameras  were  uncorrelated,  and  thus  the  uncertainty  in  the  parallax  between 
the  two  views  of  the  landmark  caused  the  depths  to  be  miscalculated.  Nevertheless,  because  the 
covariance  of  the  input  errors  was  known,  the  distribution  of  the  errors  in  the  triangulation  solution 
was  accurately  predicted. 

For  smaller  values  of  Euler  angle  noise,  the  error  distribution  is  accurately  predicted  by  the 
algorithm  presented  in  this  report.  This  is  expected,  as  the  camera  triangulation  equation  is  a  linear 
function  of  the  camera  position  but  a  nonlinear  (trigonometric)  function  of  the  camera  orientation’s 
Euler  angles.  Thus,  the  larger  the  error  in  angle,  the  less  accurate  the  Taylor  expansion  will  be. 
Furthermore,  at  larger  errors  for  angles  the  position  error  becomes  biased,  and  points  tend  to  be 
triangulated  at  a  location  closer  than  the  truth  value.  These  biases,  however,  appear  at  large  values  of 
angle  noise  standard  deviation  (10  degrees),  which  is  much  larger  than  the  gyro  noise  from  tactical- 
grade  inertial  measurement  units.  For  example,  if  triangulation  for  a  visual  landmark  is  performed  on 
pictures  taken  10  seconds  apart,  Euler  angle  standard  deviations  are  rarely  more  than  1  degree. 
Tactical-grade  inertial  measurement  units  (IMUs)  exhibit  gyro  noise  that  is  usually  on  the  order  of 

0.03-0. 1  ° /  Vhr  or  1.8-6  ° !  hr !  a/ Hz  A  navigation  system  with  0.17  Vhr  Gaussian  gyro  noise, 
sampling  the  gyro  at  100  Hz,  would  see  uncorrelated  gyro  rate  errors  of  about  0.0167  7  sec  per 
sample.  Summed  over  10  secs  (1,000  samples),  these  gyro  rate  errors  would  result  in  Euler  angle 
errors  with  standard  deviations  of  about  0.00528  degree  per  axis. 
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shows  similar  figures  for  other  grades  of  IMUs. 

Because  of  the  small  magnitude  of  random  Euler  angle  errors  from  even  inexpensive  gyroscopes, 
the  presented  algorithm  for  triangulation  is  suitable  for  triangulation  from  navigation  systems.  For 
triangulation  with  camera  attitudes  that  do  not  have  quality  attitude  measurements,  the  presented 
method  accurately  estimates  the  triangulation  error  for  angle  noise  with  a  standard  deviation  smaller 
than  5  degrees.  Figure  13  illustrates  the  accuracy  of  the  triangulation  error-estimation  in  a  different 
fashion.  Here,  the  distribution  of  Mahalanobis  distances  is  compared  to  the  ^  -distribution  for  three 
degrees  of  freedom. 

As  shown  in  Figure  13,  for  small  values  of  angle  variance  the  error  estimate  is  accurate,  and  the 
expected  error  distribution  follows  the  actual  error  distribution.  However,  as  the  standard  deviation  of 
the  Euler  angle  noise  approaches  5  degrees  for  roll,  pitch,  and  yaw,  the  error  estimate  begins  to 
degrade.  As  the  position  noise  increases  for  a  fixed  amount  of  angle  noise,  the  position  noise  has  a 
larger  weight  on  the  final  error  than  the  angle  noise  and  the  estimate  regains  its  accuracy. 


Table  4.  Comparison  of  random  noise  from  different  grades  of  gyroscopes®. 


Silicon-Vibratory 

MEMs 

Tactical-grade  IFOGs 

Aviation-Grade 

Spinning  Mass 

Random  gyro  rate  noise 

1  vVhr 

O.r/Vhr 

0.002  7  Viir 

Random  gyro  rate  noise  per 
sample  at100  Hz 

0.167  7  sec 

0.0167  7  sec 

3.3x1 0'"^  7  sec 

Random  Euler  angle  error 
standard  deviation  after  10 
seconds  (1 ,000  samples  at 

100  Hz) 

0.0528° 

0.00528° 

1.0x10"^° 

Random  Euler  angle  error 
standard  deviation  after  90 
seconds  (9,000  samples  at 

100  Hz) 

0.1584° 

0.01584° 

3.0x10'®° 
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Angle  Standard  Deviation 


Position  Standard  Deviation 


1  in 

1 

5  111 

1 

10  in 

1 

1  1  1 

Figure  12.  The  spread  of  the  triangulation  solution  values  in  the  North/East  plane.  The  x-axis  in 
each  sub-chart  represents  east  coordinates,  in  meters;  the  y-axis  represents  north  coordinates, 
in  meters.  The  measured  covariance,  represented  by  dashed  black  ellipses,  is  the  covariance 
calculated  from  the  samples.  The  expected  covariance,  represented  by  solid  red  ellipses,  is  the 
distribution  around  the  true  point  location  calculated  by  the  covariance  estimation  algorithm.  For 
small  values  of  Euler  angle  noise  variance  (1  degree  or  less),  the  predicted  distribution  is 
accurate  enough  that  the  expected  and  measured  covariance  ellipses  overlap  in  the  figure. 
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Angle  Standard  Deviation 
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10  m 
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% 


Position  Standard  Deviation 

5  m 
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Figure  13.  The  distribution  of  errors  versus  the  expected  distribution  of  errors.  The  blue  lines 
represent  observed  distributions  while  the  black  lines  represent  expected  distributions.  The 
observed  and  expected  distributions  match  up  well  for  all  cases  except  for  large  attitude  errors 
combined  with  small  camera  position  errors  (Figures  g  and  j). 


24 


6.3  PROJECTION  COVARIANCE  ESTIMATION 

The  projection  covariance  estimator  derived  in  Section  5  was  verified  using  a  Monte  Carlo 
simulation.  In  this  simulation,  a  point  with  a  noisy  location  was  projected  onto  a  camera  with  noisy 
position  and  attitude.  The  results  of  this  simulation  are  shown  in  Figure  14. 

Figure  14  shows  that  the  observed  distribution  of  uv  coordinates  follows  the  expected 
distribution.  That  is,  the  shape  of  the  error  distribution  on  the  uv-plane  is  accurately  predicted. 

To  evaluate  the  performance  of  the  error  estimate,  the  Mahalanobis  distance  is  calculated  using 
the  following  equation: 


m  = 


(48) 


Here,  is  the  covariance  matrix  calculated  using  Equation  (46),  u  and  v  are  from  the  output  of 

the  projection  estimation  Equation  (37)  given  the  noisy  inputs,  and  u  and  v  are  the  true  pixel 
coordinates. 

The  Mahalanobis  distances  for  the  u  and  v  errors  follow  a  2-dof  -distribution,  as  shown  in 
Figure  15. 

The  distribution  of  the  Mahalanobis  distances  of  the  u,v  coordinate  errors  follows  the  expected 
Chi-square  distribution.  That  is,  the  magnitude  of  error  is  accurately  predicted. 

The  simulation  results  indicate  that  the  projection  covariance  estimator  is  accurate  when 
characteristics  of  the  input  noise  are  known. 
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Figure  14.  The  results  of  the  Monte  Carlo  simulation  of  UV  projection  error. 
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0  5  10  15  20 

Mahalanobis  Distance^ 

Figure  15.  The  distribution  of  Mahalanobis  distances  for  the  Monte  Carlo  simulation  of  UV  projection 
errors. 

The  distribution  of  the  Mahalanobis  distances  of  the  uv  coordinate  errors  follows  the  expected 
Chi-square  distribution.  That  is,  the  magnitude  of  error  is  accurately  predicted. 

The  simulation  results  indicate  that  the  projection  covariance  estimator  is  accurate  when 
characteristics  of  the  input  noise  are  known. 

7.  CONCLUSION 

This  report  presented  an  algorithm  for  triangulating  the  location  of  a  visual  landmark  from  two 
camera  observations.  A  method  for  estimating  the  accuracy  (covariance)  of  the  triangulation  solution 
was  also  presented.  For  the  most  part,  the  covariance  estimation  algorithm  described  in  this  paper 
provides  an  accurate  estimate  of  the  triangulation  error.  However,  it  is  important  to  note  that  this 
algorithm  is  limited  to  applications  where  Euler  angle  noise  is  smaller  than  5  degrees.  This  is  because 
the  main  decider  of  the  accuracy  of  the  error  estimate  is  the  magnitude  of  the  Euler  angle  noise.  As 
the  magnitude  of  Euler  angle  noise  approaches  5  degrees  for  each  axis,  the  linear  approximations 
made  by  the  presented  algorithm  no  longer  accurately  predict  errors.  Thus,  for  Euler  angle  noise 
greater  than  5  degrees,  the  triangulation  results  and  error  estimates  become  un-reliable.  However, 
most  navigation  systems  have  an  Euler  angle  noise  standard  deviation  much  smaller  than  5  degrees 
(on  the  order  of  half  a  degree  for  inexpensive  microelectromechanical  systems  (MEMS)  or 
magnetometers)'®,  so  this  non-linearity  is  a  non-issue.  Therefore,  in  systems  where  the  camera’s 
attitude  is  determined  using  inexpensive  MEMS  gyros  or  better,  the  linear  triangulation  method  and 
its  associated  covariance  estimator  remain  useful  for  mapping  visual  landmarks  and  estimating  the 
position  errors. 
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